#include <std_msgs/Int32.h>

#include <matchmaker/client.h>

/**
 * @brief Example of finding, monitoring and using a service
 */
class ExampleConsumer
{
public:
  ExampleConsumer() { }
  ~ExampleConsumer() { }

  void main()
  {
    // Start a timer to find the service whenever we don't have it.
    myServiceFindTimer_ = nh_.createTimer(ros::Duration(1),
        &ExampleConsumer::findServiceTimerCB, this);

    // Spin to process ROS events, until killed.
    ros::spin();
  }

private:
  void findServiceTimerCB(const ros::TimerEvent& event)
  {
    // Don't do anything if we have a monitored service already.
    if (myServiceMon_) return;

    matchmaker::ServiceDetailsPtr service = mmClient_.findService("MadeUpService");

    if (!service) {
      ROS_INFO_STREAM("Waiting for usable service.");
      return;
    }

    ROS_INFO_STREAM("Found usable service.");
    ROS_INFO_STREAM("Subscribing to " <<  service->topic);
    myServiceSub_ = nh_.subscribe(service->topic, 1, &ExampleConsumer::myServiceMsgCB, this);

    // Set up a service status monitor, with a callback function.
    myServiceMon_ = mmClient_.monitorService(service,
        boost::bind(&ExampleConsumer::myServiceStatusCB, this, _1));
  }

  void myServiceStatusCB(const matchmaker::ServiceDetails& service)
  {
    ROS_INFO_STREAM("Service " << service.name << " status " << service.status.toString());

    if (!service.status.isUsable()) {
      ROS_WARN_STREAM("Lost service " << service.name << " on " << service.topic);
      myServiceMon_.shutdown();
      myServiceSub_.shutdown();
    }
  }

  void myServiceMsgCB(std_msgs::Int32ConstPtr msg)
  {
    ROS_INFO_STREAM("Received msg value " << msg->data << " on " << myServiceSub_.getTopic());
  }

  ros::NodeHandle nh_;

  matchmaker::Client mmClient_;

  ros::Timer myServiceFindTimer_;
  matchmaker::ServiceMonitor myServiceMon_;
  ros::Subscriber myServiceSub_;
};

int main(int argc, char ** argv)
{
  // AnonymousName init option adds changeable number to node name, so
  // we can run multiple instances of this node at once without clashes.
  ros::init(argc, argv, "consumer", ros::init_options::AnonymousName);

  ExampleConsumer example;
  example.main();
  exit(0);
}

/** \example example_consumer.cpp
 * This code illustrates how to find and monitor a simple ROS message
 * publishing service service using the matchmaker::Client. You can find it
 * within the matchmaker package, in the file src/example_consumer.cpp.
 * Once you have built the matchmaker module, you can try running it like so:
\verbatim
$ rosrun matchmaker example_consumer
\endverbatim
 *
 * Note the use of the timer callback method findServiceTimerCB to find
 * a usable service, but the method does nothing when an active monitor
 * already exists.
 *
 * If you run two of the example_providers a number of seconds apart, and
 * one or more example_consumers, you will see how the consumers switch
 * provider immediately when the one they are using becomes unavailable.
 */
